/**
 * @file    jm_frame_port.c
 * @brief   关节电机服务
 * @details This is the detail description.
 * @author  wang-haoyu
 * @date    2025年7月10日 (created)
 * @version 0.0.1
 * @par Copyright (C):
 *          All Rights Reserved.
 * @par Encoding:
 *          UTF-8
 * @par Description        :
 * 1. Hardware Descriptions:
 *      None.
 * 2. Program Architecture:
 *      None.
 * 3. File Usage:
 *      None.
 * 4. Limitations:
 *      None.
 * 5. Else:
 *      None.
 * @par Modification:
 * -# Date          : 2025年7月10日;
 * Revision         : 0.0.1;
 * Author           : wang-haoyu;
 * Contents         :
 */
 
/* =============================================================================
 *                                header files
 * ========================================================================== */
#include "jm_drv_master.h"
#define LOG_TAG "jm_drv_master"
#include "ulog.h"

/* =============================================================================
 *                              macros & constants
 * ========================================================================== */

/* =============================================================================
 *                          data structure definitions
 * ========================================================================== */
static rt_device_t jm_can_dev;     
static rt_sem_t jm_can_rx_sem;   
static rt_sem_t jm_can_unpack_sem;   
struct rt_messagequeue jm_send_mq;
static rt_timer_t jm_connect_timer;
static jm_fifo_port_t jm_fifo_port =
{
    .fifo_name = JOINT_MOTOR_RECV_FIFO_NAME,
    .fifo_size = JOINT_MOTOR_RECV_FIFO_SIZE
};

static jm_base_ctrl_param_t jm_base_ctrl_param[MAX_JM_DRV_SLAVE_NODE] = {0};
static jm_drv_info_get_t jm_drv_info_get[MAX_JM_DRV_SLAVE_NODE] = {0};
static jm_data_analys_param_t jm_data_analys_param[MAX_JM_DRV_SLAVE_NODE] = {0};
static utils_timer_t jm_drv_connect_timeout[MAX_JM_DRV_SLAVE_NODE] = {0};

/* =============================================================================
 *                         static variable definitions
 * ========================================================================== */
static rt_uint8_t jm_msg_pool[2048];
/* =============================================================================
 *                         static function definitions
 * ========================================================================== */
/*
 * 关节电机can数据接收回调函数
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
static rt_err_t jm_drv_comm_rx_callback(rt_device_t dev, rt_size_t size)
{
    if(size > 0)
        rt_sem_release(jm_can_rx_sem);
	return RT_EOK;
}

/*
 * 关节电机数据发送线程
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
static void jm_drv_send_data_thread_entry(void *paramter)
{
    struct rt_can_msg msg;
    rt_ssize_t size;
    rt_uint8_t buf[sizeof(msg)] = {0};

    while(1)
    {
        size = rt_mq_recv(&jm_send_mq, &buf, sizeof(buf), RT_WAITING_FOREVER);
         if(size > 0)
        {
            rt_memcpy(&msg,&buf,sizeof(msg));
            rt_device_write(jm_can_dev, 0, &msg, sizeof(msg));
        }
    }
}

/*
 * 关节电机数据接收线程
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
static void jm_drv_recv_data_thread_entry(void *paramter)
{
    rt_size_t recv_cnt = 0;
	struct rt_can_msg rxmsg = {0};
	while(1)
	{
		rt_sem_take(jm_can_rx_sem, RT_WAITING_FOREVER);

        rxmsg.hdr_index = -1;
		recv_cnt = rt_device_read(jm_can_dev, 0, &rxmsg, sizeof(rxmsg));
        
        if(0 < recv_cnt)
            jm_drv_fifo_write(&jm_fifo_port, &rxmsg, recv_cnt);

        rt_sem_release(jm_can_unpack_sem);
	}
}

/*
 * 关节电机数据解析线程
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
static void jm_drv_unpack_data_thread_entry(void *paramter)
{
    while(1)
    {
        rt_sem_take(jm_can_unpack_sem, RT_WAITING_FOREVER);
        jm_drv_frame_unpack(&jm_fifo_port,jm_data_analys_param,jm_drv_info_get,jm_drv_connect_timeout);
    }
}

/*
 * 电机通信超时函数
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
static void jm_drv_comm_timeout_state(void *parameter)
{
	for(int i = 0;i<MAX_JM_DRV_SLAVE_NODE;i++)
	{
		if(utils_timer_is_expired(&jm_drv_connect_timeout[i]))
			jm_drv_info_get[i].connect_sta = false;
	}
}
/* =============================================================================
 *                         global function definitions
 * ========================================================================== */

/*
 * 关节电机数据发送回调函数（对外）
 *
 * Parameter:
 *   Input:
 *      -*buffer：数据输入缓冲区
 *      -size:输入数据长度
 *   Output:
 * Returns:
 */
void jm_drv_tx_callback(const void *buffer, rt_size_t size)
{
    rt_mq_send(&jm_send_mq, buffer, size);
}

/*
 * 关节电机数据紧急发送回调函数（对外）
 *
 * Parameter:
 *   Input:
 *      -*buffer：数据输入缓冲区
 *      -size:输入数据长度
 *   Output:
 * Returns:
 */
void jm_drv_tx_urgent_callback(const void *buffer, rt_size_t size)
{
    rt_mq_urgent(&jm_send_mq, buffer, size);
}
/*
 * 关节电机数据获取回调函数（对外）
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
void jm_drv_info_get_callback(rt_uint8_t node_id,jm_drv_info_get_t *obj)
{
    rt_base_t level = rt_hw_interrupt_disable();
	*obj = jm_drv_info_get[node_id-1];
	rt_hw_interrupt_enable(level);
}

/*
 * 关节电机基础控制参数获取回调函数（对外）
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
void jm_drv_base_param_get_callback(rt_uint8_t node_id,jm_base_ctrl_param_t *obj)
{
    rt_base_t level = rt_hw_interrupt_disable();
	*obj = jm_base_ctrl_param[node_id-1];
	rt_hw_interrupt_enable(level);
}

/*
 * 关节电机注册函数
 *
 * Parameter:
 *   Input:
 *		-node_type：关节电机类型
 *      -node_id：关节电机ID
 *   Output:
 * Returns:
 *		-返回状态
 */
int joint_motor_drv_register(joint_motor_drv_model_t node_type,rt_uint8_t node_id)
{
    if((node_id > MAX_JM_DRV_SLAVE_NODE) || (node_id < 1) ||
        (node_type == MODEL_NONE) || (node_type == MODEL_INVALID))
    {
        LOG_E("Register a invalid jm slave node i");   
        return -1;
    }

    if(node_type == DAMIAO_4310)
    {
        jm_data_analys_param[node_id - 1] = JM_DRV_DATA_ANALYS_INITALIZER(DAMIAO_4310);
        jm_base_ctrl_param[node_id - 1] = JM_DRV_BASE_CTRL_INITALIZER(DAMIAO_4310);
        utils_timer_countdown_ms(&jm_drv_connect_timeout[node_id - 1],100);
    }
    else if(node_type == DAMIAO_4340)
    {
        jm_data_analys_param[node_id - 1] = JM_DRV_DATA_ANALYS_INITALIZER(DAMIAO_4340);
        jm_base_ctrl_param[node_id - 1] = JM_DRV_BASE_CTRL_INITALIZER(DAMIAO_4340);
        utils_timer_countdown_ms(&jm_drv_connect_timeout[node_id - 1],100);
    }
    return 0;
}

/*
 * 关节电机初始化函数
 *
 * Parameter:
 *   Input:
 *   Output:
 * Returns:
 */
void jm_drv_init(void)
{
    rt_err_t res;
    JM_FIFO_ENOERR_CODE status = 0;

    jm_can_dev = rt_device_find(JOINT_MOTOR_CAN_DEVICE_NAME);
	if(jm_can_dev == RT_NULL)
	{
		LOG_E("find %s failed!", JOINT_MOTOR_CAN_DEVICE_NAME);
		return ;
	}
	res = rt_device_open(jm_can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
	RT_ASSERT(res == RT_EOK);

    rt_device_set_rx_indicate(jm_can_dev, jm_drv_comm_rx_callback);

    jm_can_rx_sem = rt_sem_create(JOINT_MOTOR_RX_SEM_NAME,JOINT_MOTOR_INIT_VALUE,RT_IPC_FLAG_FIFO);
	if(jm_can_rx_sem == RT_NULL)
		LOG_E("sem creat faild");

    jm_can_unpack_sem = rt_sem_create(JOINT_MOTOR_UNPACK_SEM_NAME,JOINT_MOTOR_INIT_VALUE,RT_IPC_FLAG_FIFO);
	if(jm_can_unpack_sem == RT_NULL)
		LOG_E("sem creat faild");

    status = jm_drv_fifo_creat(&jm_fifo_port);
    if(status != 0)
    {
        LOG_E("fifo creat faild");
        return ;
    }

    res = rt_mq_init(&jm_send_mq,"jm_mq",&jm_msg_pool,16,sizeof(jm_msg_pool),RT_IPC_FLAG_PRIO);       
    if (res != RT_EOK)
    {
        LOG_E("init message queue failed.\n");
        return ;
    }
    jm_connect_timer = rt_timer_create("jm_connect_timer",jm_drv_comm_timeout_state,RT_NULL,10,RT_TIMER_FLAG_PERIODIC);

    if (jm_connect_timer != RT_NULL) 
		rt_timer_start(jm_connect_timer);

    rt_thread_t jm_recv_thread = rt_thread_create("jm_drv_recv",jm_drv_recv_data_thread_entry, RT_NULL,JOINT_MOTOR_CAN_RECV_THREAD_PARAM);
    if (jm_recv_thread != RT_NULL)
		rt_thread_startup(jm_recv_thread);
    else
        goto __exit;

    rt_thread_t jm_unpack_thread = rt_thread_create("jm_drv_unpack",jm_drv_unpack_data_thread_entry, RT_NULL,JOINT_MOTOR_CAN_UNPACK_THREAD_PARAM);
    if (jm_unpack_thread != RT_NULL)
		rt_thread_startup(jm_unpack_thread);
    else
        goto __exit;

    rt_thread_t jm_send_thread = rt_thread_create("jm_drv_send",jm_drv_send_data_thread_entry, RT_NULL,JOINT_MOTOR_CAN_SEND_THREAD_PARAM);
    if (jm_send_thread != RT_NULL)
		rt_thread_startup(jm_send_thread);
    else
        goto __exit;

    return ;

__exit:
	if(jm_recv_thread)
		rt_thread_delete(jm_recv_thread);
	if(jm_unpack_thread)
		rt_thread_delete(jm_unpack_thread);
	if(jm_send_thread)
		rt_thread_delete(jm_send_thread);
}

/*******************************************************************************
 *                                  EOF
 ******************************************************************************/